#include "JY61P.h"


// PB16-RX == imu TX
// PB15-TX == imu RX


static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
const uint32_t c_uiBaud[10] = {0, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600};
float fAcc[3], fGyro[3], fAngle[3];



// 更改 UART2 波特率的函数
void Usart2Init(uint32_t baudRate)
{
    uint32_t ibrd, fbrd;

//    switch (baudRate)
//    {
//        case UART_2_BAUD_RATE_0:
//            // 如果波特率为0，返回或处理错误
//            return;
//        case UART_2_BAUD_RATE_4800:
//            ibrd = UART_2_IBRD_32_MHZ_4800;
//            fbrd = UART_2_FBRD_32_MHZ_4800;
//            break;
//        case UART_2_BAUD_RATE_9600:
//            ibrd = UART_2_IBRD_32_MHZ_9600;
//            fbrd = UART_2_FBRD_32_MHZ_9600;
//            break;
//        case UART_2_BAUD_RATE_19200:
//            ibrd = UART_2_IBRD_32_MHZ_19200;
//            fbrd = UART_2_FBRD_32_MHZ_19200;
//            break;
//        case UART_2_BAUD_RATE_38400:
//            ibrd = UART_2_IBRD_32_MHZ_38400;
//            fbrd = UART_2_FBRD_32_MHZ_38400;
//            break;
//        case UART_2_BAUD_RATE_57600:
//            ibrd = UART_2_IBRD_32_MHZ_57600;
//            fbrd = UART_2_FBRD_32_MHZ_57600;
//            break;
//        case UART_2_BAUD_RATE_115200:
//            ibrd = UART_2_IBRD_32_MHZ_115200;
//            fbrd = UART_2_FBRD_32_MHZ_115200;
//            break;
//        case UART_2_BAUD_RATE_230400:
//            ibrd = UART_2_IBRD_32_MHZ_230400;
//            fbrd = UART_2_FBRD_32_MHZ_230400;
//            break;
//        case UART_2_BAUD_RATE_460800:
//            ibrd = UART_2_IBRD_32_MHZ_460800;
//            fbrd = UART_2_FBRD_32_MHZ_460800;
//            break;
//        case UART_2_BAUD_RATE_921600:
//            ibrd = UART_2_IBRD_32_MHZ_921600;
//            fbrd = UART_2_FBRD_32_MHZ_921600;
//            break;
//        default:
//            return; // 未知波特率，直接返回
//    }

//    
//	
////	DL_UART_Main_setClockConfig(UART_2_INST, (DL_UART_Main_ClockConfig *) &gUART_2ClockConfig);

////    DL_UART_Main_init(UART_2_INST, (DL_UART_Main_Config *) &gUART_2Config);
////    /*
////     * Configure baud rate by setting oversampling and baud rate divisors.
////     *  Target baud rate: 9600
////     *  Actual baud rate: 9598.08
////     */
////    DL_UART_Main_setOversampling(UART_2_INST, DL_UART_OVERSAMPLING_RATE_16X);

//	// 设置新的波特率
//    DL_UART_Main_setBaudRateDivisor(UART_2_INST, ibrd, fbrd);

////    /* Configure Interrupts */
////    DL_UART_Main_enableInterrupt(UART_2_INST,
////                                 DL_UART_MAIN_INTERRUPT_RX);
////    /* Setting the Interrupt Priority */
////    NVIC_SetPriority(UART_2_INST_INT_IRQN, 3);


////    DL_UART_Main_enable(UART_2_INST);
}

// 处理接收到的命令数据
void CopeCmdData(unsigned char ucData)
{
    static unsigned char s_ucData[50], s_ucRxCnt = 0;

    s_ucData[s_ucRxCnt++] = ucData;         // 存储接收到的数据并增加计数器
    if (s_ucRxCnt < 3) return;              // 如果数据小于3字节，返回
    if (s_ucRxCnt >= 50) s_ucRxCnt = 0;     // 超过缓冲区长度则重置计数器
    if (s_ucRxCnt >= 3)
    {
        if ((s_ucData[1] == '\r') && (s_ucData[2] == '\n'))  // 检查结束符
        {
            s_cCmd = s_ucData[0];           // 提取命令字符
            memset(s_ucData, 0, 50);        // 清空缓冲区
            s_ucRxCnt = 0;                  // 重置计数器
        }
        else 
        {
            // 移动数据以便接收下一组数据
            s_ucData[0] = s_ucData[1];
            s_ucData[1] = s_ucData[2];
            s_ucRxCnt = 2;
        }
    }
}

// 显示帮助信息
void ShowHelp(void)
{
    printf("\r\n************************  WIT_SDK_DEMO  ************************");
    printf("\r\n************************        HELP        ************************\r\n");
    printf("UART SEND:a\\r\\n   Acceleration calibration.\r\n");
    printf("UART SEND:m\\r\\n   Magnetic field calibration, after calibration send: e\\r\\n to indicate the end\r\n");
    printf("UART SEND:U\\r\\n   Bandwidth increase.\r\n");
    printf("UART SEND:u\\r\\n   Bandwidth reduction.\r\n");
    printf("UART SEND:B\\r\\n   Baud rate increased to 115200.\r\n");
    printf("UART SEND:b\\r\\n   Baud rate reduction to 9600.\r\n");
    printf("UART SEND:R\\r\\n   The return rate increases to 10Hz.\r\n");
    printf("UART SEND:r\\r\\n   The return rate reduction to 1Hz.\r\n");
    printf("UART SEND:C\\r\\n   Basic return content: acceleration, angular velocity, angle, magnetic field.\r\n");
    printf("UART SEND:c\\r\\n   Return content: acceleration.\r\n");
    printf("UART SEND:h\\r\\n   Help.\r\n");
    printf("******************************************************************************\r\n");
}

// 处理接收到的命令
//void CmdProcess(void)
//{
//    switch (s_cCmd)
//    {
//        case 'a':    
//            if (WitStartAccCali() != WIT_HAL_OK) 
//                printf("\r\nSet AccCali Error\r\n");
//            break;
//        case 'm':    
//            if (WitStartMagCali() != WIT_HAL_OK) 
//                printf("\r\nSet MagCali Error\r\n");
//            break;
//        case 'e':    
//            if (WitStopMagCali() != WIT_HAL_OK)
//                printf("\r\nSet MagCali Error\r\n");
//            break;
//        case 'u':    
//            if (WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK) 
//                printf("\r\nSet Bandwidth Error\r\n");
//            break;
//        case 'U':    
//            if (WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) 
//                printf("\r\nSet Bandwidth Error\r\n");
//            break;
//        case 'B':    
//            if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) 
//                printf("\r\nSet Baud Error\r\n");
//            else 
//                Usart2Init(c_uiBaud[WIT_BAUD_115200]);                                            
//            break;
//        case 'b':    
//            if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
//                printf("\r\nSet Baud Error\r\n");
//            else 
//                Usart2Init(c_uiBaud[WIT_BAUD_9600]);                                                
//            break;
//        case 'R':    
//            if (WitSetOutputRate(RRATE_10HZ) != WIT_HAL_OK) 
//                printf("\r\nSet Rate Error\r\n");
//            break;
//        case 'r':    
//            if (WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) 
//                printf("\r\nSet Rate Error\r\n");
//            break;
//        case 'C':    
//            if (WitSetContent(RSW_ACC | RSW_GYRO | RSW_ANGLE | RSW_MAG) != WIT_HAL_OK) 
//                printf("\r\nSet RSW Error\r\n");
//            break;
//        case 'c':    
//            if (WitSetContent(RSW_ACC) != WIT_HAL_OK) 
//                printf("\r\nSet RSW Error\r\n");
//            break;
//        case 'h':
//            ShowHelp();
//            break;
//    }
//    s_cCmd = 0xff;  // 重置命令
//}

// 处理接收到的命令
void CmdProcess(void)
{
    switch (s_cCmd)
    {
        case 'a':    
            // 启动加速度计校准
            if (WitStartAccCali() != WIT_HAL_OK) 
                printf("\r\nSet AccCali Error\r\n");
            break;
        case 'm':    
            // 启动磁力计校准
            if (WitStartMagCali() != WIT_HAL_OK) 
                printf("\r\nSet MagCali Error\r\n");
            break;
        case 'e':    
            // 停止磁力计校准
            if (WitStopMagCali() != WIT_HAL_OK)
                printf("\r\nSet MagCali Error\r\n");
            break;
        case 'u':    
            // 设置带宽为5Hz
            if (WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK) 
                printf("\r\nSet Bandwidth Error\r\n");
            break;
        case 'U':    
            // 设置带宽为256Hz
            if (WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) 
                printf("\r\nSet Bandwidth Error\r\n");
            break;
        case 'B':    
            // 设置UART波特率为115200
            if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) 
                printf("\r\nSet Baud Error\r\n");
            else 
                Usart2Init(c_uiBaud[WIT_BAUD_115200]);  // 重新初始化UART2
            break;
        case 'b':    
            // 设置UART波特率为9600
            if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
                printf("\r\nSet Baud Error\r\n");
            else 
                Usart2Init(c_uiBaud[WIT_BAUD_9600]);  // 重新初始化UART2
            break;
        case 'R':    
            // 设置输出速率为10Hz
            if (WitSetOutputRate(RRATE_10HZ) != WIT_HAL_OK) 
                printf("\r\nSet Rate Error\r\n");
            break;
        case 'r':    
            // 设置输出速率为1Hz
            if (WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) 
                printf("\r\nSet Rate Error\r\n");
            break;
        case 'C':    
            // 设置输出内容为加速度计、陀螺仪、角度和磁力计
            if (WitSetContent(RSW_ACC | RSW_GYRO | RSW_ANGLE | RSW_MAG) != WIT_HAL_OK) 
                printf("\r\nSet RSW Error\r\n");
            break;
        case 'c':    
            // 设置输出内容为加速度计
            if (WitSetContent(RSW_ACC) != WIT_HAL_OK) 
                printf("\r\nSet RSW Error\r\n");
            break;
        case 'h':
            // 显示帮助信息
            ShowHelp();
            break;
    }
    s_cCmd = 0xff;  // 重置命令
}


// 通过UART发送数据
void SensorUartSend(uint8_t *p_data, uint32_t uiSize)
{
    Uart2Send(p_data, uiSize);
//	  uart_send_data(Uart2, p_data, uiSize);
}

// 毫秒级延时
void Delayms(uint16_t ucMs)
{
    delay_ms(ucMs);
}

// 更新传感器数据
void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
    int i;
    for (i = 0; i < uiRegNum; i++)
    {
        switch (uiReg)
        {
            case AZ:
                s_cDataUpdate |= ACC_UPDATE;
                break;
            case GZ:
                s_cDataUpdate |= GYRO_UPDATE;
                break;
            case HZ:
                s_cDataUpdate |= MAG_UPDATE;
                break;
            case Yaw:
                s_cDataUpdate |= ANGLE_UPDATE;
                break;
            default:
                s_cDataUpdate |= READ_UPDATE;
                break;
        }
        uiReg++;
    }
}

// 自动扫描传感器
void AutoScanSensor(void)
{
    int i, iRetry;
	
    for (i = 1; i < 10; i++)
    {
		Usart2Init(c_uiBaud[i]);
        iRetry = 2;
        do
        {
            s_cDataUpdate = 0;
            WitReadReg(AX, 3);
            delay_ms(100);
            if (s_cDataUpdate != 0)
            {
                printf("%d baud find sensor\r\n\r\n", c_uiBaud[i]);
                ShowHelp();
                return;
            }
            iRetry--;
        } while (iRetry);
    }
//	OLED_ShowNum(1,12,6,1);
    printf("can not find sensor\r\n");
    printf("please check your connection\r\n");
}


void JP61P_Init(void)
{
	delay_ms(500);
	printf("\r\n********************** wit-motion normal example  ************************\r\n");
	WitInit(WIT_PROTOCOL_NORMAL, 0x50);
	WitSerialWriteRegister(SensorUartSend);
	WitRegisterCallBack(SensorDataUpdata);
	WitDelayMsRegister(Delayms);
	printf("\r\n********************** wit-motion normal example  ************************\r\n");
	AutoScanSensor();
//	OLED_ShowNum(1,12,7,1);
}


void JP61P_Read(void)
{
	int i;
	
	CmdProcess();
	if(s_cDataUpdate)
	{
		for(i = 0; i < 3; i++)
		{
			fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
			fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
			fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
		}
		if(s_cDataUpdate & ACC_UPDATE)
		{
//			printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
			s_cDataUpdate &= ~ACC_UPDATE;
		}
		if(s_cDataUpdate & GYRO_UPDATE)
		{
//			printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
			s_cDataUpdate &= ~GYRO_UPDATE;
		}
		if(s_cDataUpdate & ANGLE_UPDATE)
		{
//			printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
			s_cDataUpdate &= ~ANGLE_UPDATE;
		}
		if(s_cDataUpdate & MAG_UPDATE)
		{
//			printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
			s_cDataUpdate &= ~MAG_UPDATE;
		}
	}
}
